﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Forms;

namespace WindowsFormsApplication1
{
    public partial class Form1 : Form
    {
        public Form1()
        {
            InitializeComponent();
        }

        private void button1_Click(object sender, EventArgs e)
        {
            double[] db = { Convert.ToDouble(textBox1.Text), Convert.ToDouble(textBox2.Text) , Convert.ToDouble(textBox3.Text) };
            double[] dc = Euler_rotationvector(db);
            textBox4.AppendText(dc[0].ToString()+"&&&"+dc[1].ToString()+"&&&"+dc[2].ToString()+"\r\n");
        }
        private double[] Euler_rotationvector(double[] Ruler_xyz)
        {

            double[] T_rv ={0,0,0 };
            double[] Eulerxyz = { 0, 0, 0 };
            for (int _a2r = 0; _a2r < 3; _a2r++)
            {
                Eulerxyz[_a2r] = (Ruler_xyz[_a2r] * (3.141592653589793 / 180));//弧度 = 角度 * π  / 180
            }
            //Eulerxyz = Ruler_xyz;
            if (Eulerxyz[0] == 0 && Eulerxyz[1] == 0 && Eulerxyz[2] == 0)
            {
                return T_rv;
            }


            double[] T_angle=new double[9];
            /*N*///XYZ-旋转
            T_angle[0] = Math.Cos(Eulerxyz[1]) * Math.Cos(Eulerxyz[2]);
            T_angle[1] = Math.Cos(Eulerxyz[0]) * Math.Sin(Eulerxyz[2]) + Math.Cos(Eulerxyz[2]) * Math.Sin(Eulerxyz[0]) * Math.Sin(Eulerxyz[1]);
            T_angle[2] = Math.Sin(Eulerxyz[0]) * Math.Sin(Eulerxyz[2]) - Math.Cos(Eulerxyz[0]) * Math.Cos(Eulerxyz[2]) * Math.Sin(Eulerxyz[1]);
            /*O*/
            T_angle[3] = -1 * (Math.Cos(Eulerxyz[1]) * Math.Sin(Eulerxyz[2]));
            T_angle[4] = 1 * (Math.Cos(Eulerxyz[0]) * Math.Cos(Eulerxyz[2]) - Math.Sin(Eulerxyz[0]) * Math.Sin(Eulerxyz[1]) * Math.Sin(Eulerxyz[2]));
            T_angle[5] = 1 * (Math.Cos(Eulerxyz[2]) * Math.Sin(Eulerxyz[0]) + Math.Cos(Eulerxyz[0]) * Math.Sin(Eulerxyz[1]) * Math.Sin(Eulerxyz[2]));
            /*A*/
            T_angle[6] = Math.Sin(Eulerxyz[1]);
            T_angle[7] = -1 * (Math.Cos(Eulerxyz[1]) * Math.Sin(Eulerxyz[0]));
            T_angle[8] = 1 * (Math.Cos(Eulerxyz[0]) * Math.Cos(Eulerxyz[1]));
            double rotsum = T_angle[0] + T_angle[4] + T_angle[8] - 1;
            double alpha = Math.Acos(rotsum / 2);
            double theta = 0;
            if (Eulerxyz[0] >= 0)
                theta = alpha;
            else
                theta = 2 * 3.141592653589793 - alpha;
            double my = 1 / (2 * Math.Sin(theta));
            double rx = my * (T_angle[5] - T_angle[7]) * theta;
            double ry = my * (T_angle[6] - T_angle[2]) * theta;
            double rz = my * (T_angle[1] - T_angle[3]) * theta;
            T_rv[0] = rx * 180 / 3.1415926;
            T_rv[1] = ry * 180 / 3.1415926;
            T_rv[2] = rz * 180 / 3.1415926;
            return T_rv;
        }
    }
}
